package com.brokendust.client.api.util;

public class LocationUtil {
    public static double[] rotateXYZ(double rotateX, double rotateY, double rotateZ, double[] inputLocation, double distance) {
        double[] rX = rotateX(rotateX, inputLocation, distance);
        double[] rY = rotateY(rotateY, rX, 0.0);
        double[] rZ = rotateZ(rotateZ, rY, 0.0);
        return rZ;
    }

    public static double[] rotateX(double rotateX, double[] inputLocation, double distance) {
        double cosX = Math.cos(Math.toRadians(rotateX));
        double sinX = Math.sin(Math.toRadians(rotateX));
        double cosY = 1.0;
        double sinY = 0.0;
        double cosZ = 1.0;
        double sinZ = 0.0;
        double nX = inputLocation[0] + distance;
        double nY = inputLocation[1];
        double nZ = inputLocation[2];
        double r11 = cosY * cosZ - sinX * sinY * sinZ;
        double r12 = sinY * cosZ + sinX * cosY * sinZ;
        double r13 = cosY * sinZ + sinX * sinY * cosZ;
        double r14 = sinY * sinZ - sinX * cosY * cosZ;
        double rX = r11 * nX - cosX * sinZ * nY + r12 * nZ;
        double rY = r13 * nX + cosX * cosZ * nY + r14 * nZ;
        double rZ = -cosX * sinY * nX + sinX * nY + cosX * cosY * nZ;
        return new double[]{rX, rY, rZ};
    }

    public static double[] rotateY(double rotateY, double[] inputLocation, double distance) {
        double cosX = 1.0;
        double sinX = 0.0;
        double cosY = Math.cos(Math.toRadians(rotateY));
        double sinY = Math.sin(Math.toRadians(rotateY));
        double cosZ = 1.0;
        double sinZ = 0.0;
        double nX = inputLocation[0] + distance;
        double nY = inputLocation[1];
        double nZ = inputLocation[2];
        double r11 = cosY * cosZ - sinX * sinY * sinZ;
        double r12 = sinY * cosZ + sinX * cosY * sinZ;
        double r13 = cosY * sinZ + sinX * sinY * cosZ;
        double r14 = sinY * sinZ - sinX * cosY * cosZ;
        double rX = r11 * nX - cosX * sinZ * nY + r12 * nZ;
        double rY = r13 * nX + cosX * cosZ * nY + r14 * nZ;
        double rZ = -cosX * sinY * nX + sinX * nY + cosX * cosY * nZ;
        return new double[]{rX, rY, rZ};
    }

    public static double[] rotateZ(double rotateZ, double[] inputLocation, double distance) {
        double cosX = 1.0;
        double sinX = 0.0;
        double cosY = 1.0;
        double sinY = 0.0;
        double cosZ = Math.cos(Math.toRadians(rotateZ));
        double sinZ = Math.sin(Math.toRadians(rotateZ));
        double nX = inputLocation[0] + distance;
        double nY = inputLocation[1];
        double nZ = inputLocation[2];
        double r11 = cosY * cosZ - sinX * sinY * sinZ;
        double r12 = sinY * cosZ + sinX * cosY * sinZ;
        double r13 = cosY * sinZ + sinX * sinY * cosZ;
        double r14 = sinY * sinZ - sinX * cosY * cosZ;
        double rX = r11 * nX - cosX * sinZ * nY + r12 * nZ;
        double rY = r13 * nX + cosX * cosZ * nY + r14 * nZ;
        double rZ = -cosX * sinY * nX + sinX * nY + cosX * cosY * nZ;
        return new double[]{rX, rY, rZ};
    }
}

